#include <digitalWriteFast.h>
/*
  MARC 5.0
 Language: Wiring/Arduino
 
Arduino code to control:
-motor speed
-encoder interaction
-sonar interaction
-bumper / emergency button

This is the primary way the robot interacts with the world, and forwards a huge amount of
data to the BeagleBoard which processes it
 
[CREDIT] - Public Domain code from Tom Igoe and Scott Fitzgerald for basic serial interaction
from - http://www.arduino.cc/en/Tutorial/SerialCallResponse
[CREDIT] - simultanous clock output via software interupt code
from - http://jeremyblum.com/2010/09/05/driving-5-speakers-simultaneously-with-an-arduino/
 
 (C) copyright 2010 Sam Erb
 This code is for SDP '10-'11 team MARC 5.0
 
Pin Mappings (Pin - short description [VAR NAME]
Analog:
sensor:
	A0 - Sonar Sensor 0 (front) [SonarRead0]
	A1 - Sonar Sensor 1 (back)  [SonarRead1]
	A2 - Sonar Sensor 2 (left)  [SonarRead2]
	A3 - Sonar Sensor 3 (right) [SonarRead3]
	A4
	A5
critical:
	A0
	A1
	A2
	A3
	A4
	A5
Digital: (using timer 2 so we have no PWM functionality)
sensor:
	0 XX DONT USE XX - serial RX/TX
	1 XX DONT USE XX - serial RX/TX
	2 - encoder interrupt 0 [EncoderIn0]
	3 - encoder interrupt 1 [EncoderIn1]
	4
	5
	6 
	7 - Sonar Trigger [SonarOUTPUT]
	8
	9
	10
	11
	12
	13
critical:
	0 XX DONT USE XX - serial RX/TX
	1 XX DONT USE XX - serial RX/TX
	2
	3
	4 - Emergency stop 0 [Emergency0]
	5 - Motor Power [MotorPowerSwitch]
	6 - Emergency stop 1 [Emergency1]
	7
	8 - On switch (must be pressed to start interacting) [TurnOnButton]
	9 - Motor 1 speed output [MotorOut0]
	10 - Motor 1 speed output [MotorOut1]
	11  - Motor Direction 0 [MotorDirection0]
	12  - Motor Direction 1 [MotorDirection1]
	13 - not moving light (emergency triggered or have not started moving yet) [Light0]

Serial Inputs:
both:
	"S" - will reply with "arduinoS" or "arduinoC" ie. hello world
sensor:
	"A" - will take and return sonar reading 
		-format: S0000|1111|2222|3333 where the numbers are the sonar readings in 0-1023 -> 0-5V
	"E" - return the encoder data - format returned: AAAAA|BBBBB|\r\n
critical:
	"MAAAAA|BBBBB" - set motor speed where AAAAA is the speed of motor 1, BBBBB is the speed of motor 2
		-is a value from 0 to 32767(int), 1 is fastest (~80MPH) and 32767 is slowest, see details below
		-a value less than maxMotorSpeed ie. 0 will trigger a stop
	"I" - disable emergency shut off - used for testing
	"F" & "R" - motor forward and reverse, controls both
	
Calculating Motor Speed:
Send value X as desired speed
This means that the arduino will send a square wave to the motor every 16us*x*2 seconds (the x2 is because the input is toggled every 64us*x)
if we have the motor on max speed, this will result in a step of 1.8/8 deg (8 is the microstep size)
and if we have have a wheel of D=6in, then we will move pi*6in every 360 deg
therefore our forumla for inches/sec from X is: (((1/(16us*x*2))*1.8/8)/360)*pi*6 , to convert that to mph, multiply by 0.05681818
Some values:
x=1 => 20.91mph (way to fast)
-----Upper Bound ----
x=4 => 5.22mph (still to fast)


MOTOR IS TIED TO OFF WHEN STOPPED
	
 */
 
 /*									*****							*/
 /////////////////////////////////MOTOR VAR's ////////////////////////
 /*									*****							*/
 /* Timer reload value, globally available */
unsigned int tcnt2;//the 2 is for timer 2

/* Toggle HIGH or LOW digital write */
int MotorToggle0 = 0;
int MotorToggle1 = 0;

/* Keep track of when each note needs to be switched */
int MotorCount0 = 0;
int MotorCount1 = 0;

/* MOTOR Output Pins */
#define MotorOut0 9
#define MotorOut1 10
#define MotorPowerSwitch 5

//defining the motor speeds
#define maxMotorSpeed 4
#define MINSPEED 600
int MotorSpeed0 = 0;
int MotorSpeed1 = 0;

//define the motor direction pin/value
#define MotorDirection0 11
#define MotorDirection1 12
int MotorDir = 0;//0 forward, 1 - backward
 
 /*									*****							*/
 /////////////////////////////////ENCODER VAR's ////////////////////////
 /*									*****							*/
#define EncoderIn0 2
#define EncoderIn1 3
int encoderCount0 = 0;
int encoderCount1 = 0;


 /*									*****							*/
 /////////////////////////////////SONAR VAR's ////////////////////////
 /*									*****							*/
int SonarRead0 = A0;    // select the input pin for the potentiometer
int SonarRead1 = A1;
int SonarRead2 = A2;
int SonarRead3 = A3;
int SonarVal0 = 0;  // variables to store the value coming from the sensor
int SonarVal1 = 0;
int SonarVal2 = 0;
int SonarVal3 = 0;
int SonarOUTPUT = 7;

 /*									*****							*/
 //////////////////////////////EMERGENCY VAR's ///////////////////////
 /*									*****							*/
#define Emergency0 4
#define Emergency1 6
int disableEmergency = 1;

 /*									*****							*/
 /////////////////////////////////MISC VAR's  ////////////////////////
 /*									*****							*/                       
char serInString[100];  // array that will hold the different bytes of the string. 100=100characters;
int serLength=0;
int JustEmergencied = 0;
#define TurnOnButton 8
#define Light0 13



 /*									*****							*/
 /////////////////////////////////INITIAL SETUP //////////////////////
 /*									*****							*/
void setup()
{
	//SETUP THE SOFTWARE INTERUPT
	/* First disable the timer overflow interrupt*/
	TIMSK2 &= ~(1<<TOIE2);
	/* Configure timer2 in normal mode (no PWM) */
	TCCR2A &= ~((1<<WGM21) | (1<<WGM20));
	TCCR2B &= ~(1<<WGM22);
	/* Select clock source: internal I/O clock */
	ASSR &= ~(1<<AS2);
	/* Disable Compare Match A interrupt (only overflow) */
	TIMSK2 &= ~(1<<OCIE2A);
	/* Configure the prescaler to CPU clock divided by 128 */
	TCCR2B |= (1<<CS22)  | (1<<CS20); // Set bits
	TCCR2B &= ~(1<<CS21);             // Clear bit
	/* We need to calculate a proper value to load the counter.
	* The following loads the value 248 into the Timer 2 counter
	* The math behind this is:
	* (Desired period) = 8us.
	* (CPU frequency) / (prescaler value) = 125000 Hz -> 8us.
	* (desired period) / 8us = 2.
	* MAX(uint8) - 2 = 254;
	*/
	/* Save value globally for later reload in ISR */
	tcnt2 = 254;
	/* Finally load end enable the timer */
	TCNT2 = tcnt2;
	TIMSK2 |= (1<<TOIE2);
	//Configure I/O Pin Directions
	pinModeFast(MotorOut0,    OUTPUT);
	pinModeFast(MotorOut1,    OUTPUT);

	Serial.begin(115200); // start serial port at 115200 bps
	
	pinMode(SonarOUTPUT, OUTPUT);//make the sonar digital pin an output
	
	//emergency inputs
	pinMode(Emergency0, INPUT);
	pinMode(Emergency1, INPUT);

	//setup the light
	pinMode(Light0, OUTPUT);
	
	//attach the interupts to read in the encoders
	attachInterrupt(0, encoderRead0, RISING);
	attachInterrupt(1, encoderRead1, RISING);
	
	//setup initial motor direction
	pinMode(MotorDirection0, OUTPUT);
	pinMode(MotorDirection1, OUTPUT);
	digitalWrite(MotorDirection0, LOW);
	digitalWrite(MotorDirection1, HIGH);
	
	//turn the motor on
	pinMode(MotorPowerSwitch, OUTPUT);
	digitalWrite(MotorPowerSwitch, HIGH);
	
	//on button
	pinMode(TurnOnButton,INPUT);

	//when the program starts up, wait for a high reading on the on switch
	startup_loop();
	
}

 /*									*****							*/
 /////////////////////////////////PRIMARY LOOP  //////////////////////
 /*									*****							*/
void loop()
{
	checkEmergencys();
	
	//read the serial port and create a string out of what you read
	if(Serial.available()) {  
		readSerialString(serInString);
		// debug echo back - printSerialString(serInString);
		processSerialString(serInString);
		delay(10);//let ADC reciever recover
	}
	//delay(10); why do we need this?
}

//startup loop - wait for on button before starting
void startup_loop(){
	digitalWrite(Light0,HIGH);
	int start=1;
	while (start==0){ 
		if (digitalRead(TurnOnButton) == HIGH){
			start=1;
			char stop[ ] = "STARTUP";
			printSerialString(stop);
			Serial.print("\r\n");
			delayMicroseconds(200);
		}
	}

	digitalWrite(Light0,LOW);
}

 /*									*****							*/
 /////////////////////////////////GENERAL FUNCTIONS  /////////////////
 /*									*****							*/

 
//read in serial string
void readSerialString (char *strArray) {
	int i = 0;
	if(Serial.available()) {    
		while(strArray[i] != 0) {
			strArray[i] = 0;                  // flush the content
			i++;          
		}
		i=0;
		//printString("reading Serial String: ");  //optional: for confirmation
		while (Serial.available()){            
			strArray[i] = Serial.read();
			i++;
			//Serial.print(strArray[(i-1)],BYTE);         //optional: for confirmation
			delayMicroseconds(200);
		}
		serLength = i;
		//printNewLine();                          //optional: for confirmation
	}
}


//Serial print a string
void printSerialString(char *strArray) {
     int i=0;
     if (strArray[i] != 0) {     
         while(strArray[i] != 0) {
           
            Serial.print( strArray[i] ,BYTE);
            //strArray[i] = 0;                  // optional: flush the content
            i++;          
         }
     }
}


//Process String
void processSerialString(char *strArray) {
	int i=0;
	//if we get an S, send back a "arduino"... just a simple hello
	char S = 'S';
	if (strArray[0] == S){
		char hello[ ] = "arduino";
		printSerialString(hello);
		Serial.print("\r\n");
	}
	//if we get an A read and send back the sonar data
	char A = 'A';
	if (strArray[0] == A){
		SonarRead();
	}
	//if we get an M get those motors spinning - format: MAAAAA|BBBBB
	char M = 'M';
	if (strArray[0] == M){
		String MotorSerial = String(strArray);
		String MotorT0 = MotorSerial.substring(1,6);
		String MotorT1 = MotorSerial.substring(7);
		char MotorT0C[6];
		char MotorT1C[6];
		MotorT0.toCharArray(MotorT0C,sizeof(MotorT0C));
		MotorT1.toCharArray(MotorT1C,sizeof(MotorT1C));
		MotorSpeed0=atoi(MotorT0C);
		int stopped = 0;
		if (MotorSpeed0 >= MINSPEED || MotorSpeed0 == 0){
			MotorSpeed0 = 0;
			stopped++;
		}
		MotorSpeed1=atoi(MotorT1C);
		if (MotorSpeed1 >= MINSPEED || MotorSpeed1 == 0){
			MotorSpeed1 = 0;
			stopped++;
		}
		if (stopped == 2){
			digitalWrite(MotorPowerSwitch, HIGH);
		}else{
			digitalWrite(MotorPowerSwitch, LOW);
		}
		/*debug Serial.print(MotorSpeed0);
		Serial.print("\r\n");
		Serial.print(MotorSpeed1);
		Serial.print("\r\n");*/
	}
	//disable emergency
	char I = 'I';
	if (strArray[0] == I){
		disableEmergency = 1 - disableEmergency;
	}
	//send out the encoder data
	char E = 'E';
	if (strArray[0] == E){
		Serial.print(makeStrProperLen(encoderReturn(0),5)+"|");
		Serial.print(makeStrProperLen(encoderReturn(1),5)+"|\r\n"); 
	}
	//motor direction
	char F = 'F';
	char R = 'R';
	if  (strArray[0] == F){
		MotorDir = 0;
		digitalWrite(MotorDirection0, HIGH);
	}
	if  (strArray[0] == R){
		MotorDir = 0;
		digitalWrite(MotorDirection0, LOW);
	}
	if  (strArray[1] == F){
		MotorDir = 0;
		digitalWrite(MotorDirection1, LOW);
	}
	if  (strArray[1] == R){
		MotorDir = 0;
		digitalWrite(MotorDirection1, HIGH);
	}
}

//make a INT into a proper length string i.e. "00123"=makeStrProperLen(123,5)
String makeStrProperLen(int input,int length){
  String str = String(input);
  String zero = "0";
  while (str.length() < length){
    str = zero + str;
  }
  return str;
}


 /*									*****							*/
 /////////////////////////////////SONAR   FUNCTIONS  /////////////////
 /*									*****							*/

//read in the sonar values
void SonarRead(){
  digitalWrite(SonarOUTPUT, HIGH);
  delayMicroseconds(40);
  digitalWrite(SonarOUTPUT, LOW);
  delay(49);
  SonarVal0 = analogRead(SonarRead0);
  Serial.print("S");
  Serial.print(makeStrProperLen(SonarVal0,4)+"|"); 
  delay(49);
  SonarVal1 = analogRead(SonarRead1);
  Serial.print(makeStrProperLen(SonarVal1,4)+"|"); 
  delay(49);
  SonarVal2 = analogRead(SonarRead2);
  Serial.print(makeStrProperLen(SonarVal2,4)+"|"); 
  delay(49);
  SonarVal3 = analogRead(SonarRead3);
  Serial.print(makeStrProperLen(SonarVal3,4)+"|\r\n"); 
}

 /*									*****							*/
 /////////////////////////////MOTOR CONTROL FUNCTIONS  ///////////////
 /*									*****							*/
 
/* Install the Interrupt Service Routine (ISR) for Timer2.  */
ISR(TIMER2_OVF_vect) {
	/* Reload the timer */
	TCNT2 = tcnt2;

	MotorCount0++; 
	MotorCount1++;
	
	if (MotorSpeed0 >= maxMotorSpeed){//we are in the acceptable motor range
		if (MotorCount0 == MotorSpeed0) {//we need to change the value every half cycle
			digitalWriteFast(MotorOut0, MotorToggle0 == 0 ? HIGH : LOW);
			MotorToggle0 = ~MotorToggle0;
			MotorCount0 = 0;
		}
	}else{//just reset the motor counter for now... just chilling :)
		MotorCount0 = 0;
	}
	if (MotorSpeed1 >= maxMotorSpeed){//we are in the acceptable motor range
		if (MotorCount1 == MotorSpeed1) {//we need to change the value every half cycle
			digitalWriteFast(MotorOut1, MotorToggle1 == 0 ? HIGH : LOW);
			MotorToggle1 = ~MotorToggle1;
			MotorCount1 = 0;
		}
	}else{//just reset the motor counter for now... just chilling :)
		MotorCount1 = 0;
	}
}

 /*									*****							*/
 /////////////////////////////ENCODER FUNCTIONS  ///////////////
 /*									*****							*/

//read in the encoder values on RISING interupts
void encoderRead0(){
	encoderCount0++;
}

void encoderRead1(){
	encoderCount1++;
}

int encoderReturn(int encoder){
	if (encoder==0){
		int temp = encoderCount0;
		encoderCount0 = 0;
		return temp;
	}else{
		int temp = encoderCount1;
		encoderCount1 = 0;
		return temp;
	}
}

 /*									*****							*/
 ////////////////////////////////EMERGENCY FUNCTIONS  ////////////////
 /*									*****							*/ 
//checks and see if we have a emergency

int readHighThenWait(int pin){
	if (digitalRead(pin) == HIGH){
		delay(5);
		if (digitalRead(pin) == HIGH){
			return HIGH;
		}
	}
	return LOW;
}

void checkEmergencys(){
	int callEmergency = 0;
	if (digitalRead(Emergency0) == LOW){
		callEmergency++;
	}
	if (digitalRead(Emergency1) == HIGH){
		callEmergency++;
	}
	if (callEmergency>0){
		if (disableEmergency == 0){
			fullStop();
			JustEmergencied = 1;
		}
		digitalWrite(Light0,HIGH);
	}/*else{
		JustEmergencied=0;
		digitalWrite(Light0,LOW);
	}*/
	//wait untill we have pressed the start button before we do anything
	if (JustEmergencied == 1){
		MotorSpeed0 = 0;
		MotorSpeed1 = 0;
		if (digitalRead(TurnOnButton) == HIGH){
			digitalWrite(Light0,LOW);
			JustEmergencied=0;
			char stop[ ] = "FSOVERRIDE";
			printSerialString(stop);
			Serial.print("\r\n");
			delayMicroseconds(200);
		}
	}
}

//stops the robot and tells everyone about it
void fullStop(){
	//stop the motors
	MotorSpeed0 = 0;
	MotorSpeed1 = 0;
	//report out to BB
	if (JustEmergencied==0){
		char stop[ ] = "FS";
		printSerialString(stop);
		Serial.print("\r\n");
		delayMicroseconds(200);
	}
}

